package org.checkboo.math;



public class Matrix33f {

	/**
	 * Values of matrix
	 */
	private float v11,v12,v13;
	private float v21,v22,v23;
	private float v31,v32,v33;
	
	public Matrix33f(float v11, float v12, float v13, float v21, float v22,
			float v23, float v31, float v32, float v33) {
		this.v11 = v11;
		this.v12 = v12;
		this.v13 = v13;
		this.v21 = v21;
		this.v22 = v22;
		this.v23 = v23;
		this.v31 = v31;
		this.v32 = v32;
		this.v33 = v33;
	}
	
	public Matrix33f(){
		identity();
	}
	
	public Matrix33f(Matrix33f matrix){
		this.v11 = matrix.v11;
		this.v12 = matrix.v12;
		this.v13 = matrix.v13;
		this.v21 = matrix.v21;
		this.v22 = matrix.v22;
		this.v23 = matrix.v23;
		this.v31 = matrix.v31;
		this.v32 = matrix.v32;
		this.v33 = matrix.v33;
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied matrix
	 * @param second matrix to multiply by
	 * @return <code>this</code> matrix
	 */
	public Matrix33f multiply(Matrix33f second){
		Matrix33f temp = new Matrix33f(this);
		
		this.v11 = temp.getV11()*second.getV11() + temp.getV12()*second.getV21() + temp.getV13()*second.getV31();
		this.v12 = temp.getV11()*second.getV12() + temp.getV12()*second.getV22() + temp.getV13()*second.getV32();
		this.v13 = temp.getV11()*second.getV13() + temp.getV12()*second.getV23() + temp.getV13()*second.getV33();
		
		this.v21 = temp.getV21()*second.getV11() + temp.getV22()*second.getV21() + temp.getV23()*second.getV31();
		this.v22 = temp.getV21()*second.getV12() + temp.getV22()*second.getV22() + temp.getV23()*second.getV32();
		this.v23 = temp.getV21()*second.getV13() + temp.getV22()*second.getV23() + temp.getV23()*second.getV33();
		
		this.v31 = temp.getV31()*second.getV11() + temp.getV32()*second.getV21() + temp.getV33()*second.getV31();
		this.v32 = temp.getV31()*second.getV12() + temp.getV32()*second.getV22() + temp.getV33()*second.getV32();
		this.v33 = temp.getV31()*second.getV13() + temp.getV32()*second.getV23() + temp.getV33()*second.getV33();
		
		return this;
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied scalar value
	 * @param scalar value to multiply by
	 * @return <code>this</code> matrix
	 */
	public Matrix33f multiply(float scalar){
		this.v11 *= scalar;
		this.v12 *= scalar;
		this.v13 *= scalar;
		this.v21 *= scalar;
		this.v22 *= scalar;
		this.v23 *= scalar;
		this.v31 *= scalar;
		this.v32 *= scalar;
		this.v33 *= scalar;
		
		return this;
	}
	
	public Vector3f multiply(Vector3f vector){
		Matrix33f tmp = new Matrix33f(this);
		tmp.multiply(new Matrix33f(vector.getX(), 0, 0, vector.getY(), 0, 0, vector.getZ(), 0, 0));
		return new Vector3f(tmp.v11, tmp.v21, tmp.v31);
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied matrix and stores it in result
	 * @param second matrix to multiply by
	 */
	public void multiplyRes(Matrix33f second, Matrix33f result){		
		result.v11 = this.getV11()*second.getV11() + this.getV12()*second.getV21() + this.getV13()*second.getV31();
		result.v12 = this.getV11()*second.getV12() + this.getV12()*second.getV22() + this.getV13()*second.getV32();
		result.v13 = this.getV11()*second.getV13() + this.getV12()*second.getV23() + this.getV13()*second.getV33();
		
		result.v21 = this.getV21()*second.getV11() + this.getV22()*second.getV21() + this.getV23()*second.getV31();
		result.v22 = this.getV21()*second.getV12() + this.getV22()*second.getV22() + this.getV23()*second.getV32();
		result.v23 = this.getV21()*second.getV13() + this.getV22()*second.getV23() + this.getV23()*second.getV33();
		
		result.v31 = this.getV31()*second.getV11() + this.getV32()*second.getV21() + this.getV33()*second.getV31();
		result.v32 = this.getV31()*second.getV12() + this.getV32()*second.getV22() + this.getV33()*second.getV32();
		result.v33 = this.getV31()*second.getV13() + this.getV32()*second.getV23() + this.getV33()*second.getV33();
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied scalar and stores it in result
	 * @param scalar value to multiply by
	 */
	public void multiplyRes(float scalar, Matrix33f result){
		result.v11 = this.v11*scalar;
		result.v12 = this.v12*scalar;
		result.v13 = this.v13*scalar;
		result.v21 = this.v21*scalar;
		result.v22 = this.v22*scalar;
		result.v23 = this.v23*scalar;
		result.v31 = this.v31*scalar;
		result.v32 = this.v32*scalar;
		result.v33 = this.v33*scalar;
	}
	
	/**
	 * Adds supplied matrix to <code>this</code> matrix
	 * @param second matrix to add
	 * @return <code>this</code> matrix
	 */
	public Matrix33f add(Matrix33f second){
		this.v11 += second.getV11();
		this.v12 += second.getV12();
		this.v13 += second.getV13();
		this.v21 += second.getV21();
		this.v22 += second.getV22();
		this.v23 += second.getV23();
		this.v31 += second.getV31();
		this.v32 += second.getV32();
		this.v33 += second.getV33();
		
		return this;
	}
	
	/**
	 * Adds supplied matrix to <code>this</code> matrix and stores it in result
	 * @param second matrix to add
	 */
	public void addRes(Matrix33f second, Matrix33f result){
		result.setV11(this.getV11()+second.getV11());
		result.setV12(this.getV12()+second.getV12());
		result.setV13(this.getV13()+second.getV13());
		result.setV21(this.getV21()+second.getV21());
		result.setV22(this.getV22()+second.getV22());
		result.setV23(this.getV23()+second.getV23());
		result.setV31(this.getV31()+second.getV31());
		result.setV32(this.getV32()+second.getV32());
		result.setV33(this.getV33()+second.getV33());
	}
	
	/**
	 * Subtracts supplied matrix from <code>this</code> matrix
	 * @param second matrix to subtract
	 * @return <code>this</code> matrix
	 */
	public Matrix33f subtract(Matrix33f second){
		this.v11 -= second.getV11();
		this.v12 -= second.getV12();
		this.v13 -= second.getV13();
		this.v21 -= second.getV21();
		this.v22 -= second.getV22();
		this.v23 -= second.getV23();
		this.v31 -= second.getV31();
		this.v32 -= second.getV32();
		this.v33 -= second.getV33();
		
		return this;
	}
	
	/**
	 * Subtracts supplied matrix from <code>this</code> matrix and stores it in result
	 * @param second matrix to subtract
	 */
	public void subtractRes(Matrix33f second, Matrix33f result){
		result.setV11(this.getV11()-second.getV11());
		result.setV12(this.getV12()-second.getV12());
		result.setV13(this.getV13()-second.getV13());
		result.setV21(this.getV21()-second.getV21());
		result.setV22(this.getV22()-second.getV22());
		result.setV23(this.getV23()-second.getV23());
		result.setV31(this.getV31()-second.getV31());
		result.setV32(this.getV32()-second.getV32());
		result.setV33(this.getV33()-second.getV33());
	}
	
	/**
	 * Stores values in target array, ordered by rows or columns
	 * @param target array to store values in
	 * @param rows true-get by rows  false-get by columns
	 */
	public void getValues(float[] target, boolean rows){
		if(target.length == 9){
			if(rows){
				target[0] = this.v11;
				target[1] = this.v12;
				target[2] = this.v13;
				target[3] = this.v21;
				target[4] = this.v22;
				target[5] = this.v23;
				target[6] = this.v31;
				target[7] = this.v32;
				target[8] = this.v33;
			}
			else{
				target[0] = this.v11;
				target[1] = this.v21;
				target[2] = this.v31;
				target[3] = this.v12;
				target[4] = this.v22;
				target[5] = this.v32;
				target[6] = this.v13;
				target[7] = this.v23;
				target[8] = this.v33;
				}
			}
		else{
			System.out.println("Target array must have length of 9!!!");
		}
	}
	
	/**
	 * Computes determinant of <code>this</code> matrix
	 * @return value of determinant
	 */
	public float determinant(){
		float determinant = (this.v11*this.v22*this.v33)+(this.v21*this.v32*this.v13)+(this.v31*this.v12*this.v23);
		determinant -= (this.v13*this.v22*this.v31)+(this.v23*this.v32*this.v11)+(this.v33*this.v12*this.v21);
		
		return determinant;
	}
	
	/**
	 * Transposes <code>this</code> matrix
	 * @return transposed matrix
	 */
	public Matrix33f transpose(){
		Matrix33f temp = new Matrix33f(this);
		
		this.v11 = temp.v11;
		this.v12 = temp.v21;
		this.v13 = temp.v31;
		this.v21 = temp.v12;
		this.v22 = temp.v22;
		this.v23 = temp.v32;
		this.v31 = temp.v13;
		this.v32 = temp.v23;
		this.v33 = temp.v33;
		
		return this;
	}
	
	/**
	 * Transposes <code>this</code> matrix and stores it in result
	 * @param result matrix to store in
	 */
	public void transposeRes(Matrix33f result){
		result.v11 = this.v11;
		result.v12 = this.v21;
		result.v13 = this.v31;
		result.v21 = this.v12;
		result.v22 = this.v22;
		result.v23 = this.v32;
		result.v31 = this.v13;
		result.v32 = this.v23;
		result.v33 = this.v33;
	}
	
	/**
	 * Computes inverse matrix to <code>this</code> matrix
	 * @return <code>this</code> inversed matrix
	 */
	public Matrix33f inverse(){
		float det = this.determinant();
		if(det == 0)
			this.zero();
		else{
			Matrix33f temp = new Matrix33f(this);
			
			this.v11 = (temp.v22*temp.v33 - temp.v23*temp.v32)/det;
			this.v12 = -(temp.v21*temp.v33 - temp.v23*temp.v31)/det;
			this.v13 = (temp.v21*temp.v32 - temp.v22*temp.v31)/det;
			this.v21 = -(temp.v12*temp.v33 - temp.v13*temp.v32)/det;
			this.v22 = (temp.v11*temp.v33 - temp.v13*temp.v31)/det;
			this.v23 = -(temp.v11*temp.v32 - temp.v12*temp.v31)/det;
			this.v31 = (temp.v12*temp.v23 - temp.v13*temp.v22)/det;
			this.v32 = -(temp.v11*temp.v23 - temp.v13*temp.v21)/det;
			this.v33 = (temp.v11*temp.v22 - temp.v12*temp.v21)/det;	
			this.transpose();
		}
		return this;
	}
	
	/**
	 * Computes inverse matrix to <code>this</code> matrix and stores it in result
	 * @param result
	 */
	public void inverseRes(Matrix33f result){
		float det = this.determinant();
		if(det == 0)
			result.zero();
		else{			
			result.v11 = (this.v22*this.v33 - this.v23*this.v32)/det;
			result.v12 = -(this.v21*this.v33 - this.v23*this.v31)/det;
			result.v13 = (this.v21*this.v32 - this.v22*this.v31)/det;
			result.v21 = -(this.v12*this.v33 - this.v13*this.v32)/det;
			result.v22 = (this.v11*this.v33 - this.v13*this.v31)/det;
			result.v23 = -(this.v11*this.v32 - this.v12*this.v31)/det;
			result.v31 = (this.v12*this.v23 - this.v13*this.v22)/det;
			result.v32 = -(this.v11*this.v23 - this.v13*this.v21)/det;
			result.v33 = (this.v11*this.v22 - this.v12*this.v21)/det;
			result.transpose();
		}
	}
	
	/**
	 * Sets values of <code>this</code> matrix to values of supplied matrix
	 * @param matrix supplied matrix
	 */
	public void set(Matrix33f matrix){
		this.v11 = matrix.v11;
		this.v12 = matrix.v12;
		this.v13 = matrix.v13;
		this.v21 = matrix.v21;
		this.v22 = matrix.v22;
		this.v23 = matrix.v23;
		this.v31 = matrix.v31;
		this.v32 = matrix.v32;
		this.v33 = matrix.v33;
	}
	
	/**
	 * Sets values of <code>this</code> matrix to zeros
	 */
	public void zero(){
		this.v11 = 0;
		this.v12 = 0;
		this.v13 = 0;
		this.v21 = 0;
		this.v22 = 0;
		this.v23 = 0;
		this.v31 = 0;
		this.v32 = 0;
		this.v33 = 0;
	}
	
	/**
	 * Sets <code>this</code> matrix to identity matrix
	 */
	public void identity(){
		this.v11 = 1;
		this.v12 = 0;
		this.v13 = 0;
		this.v21 = 0;
		this.v22 = 1;
		this.v23 = 0;
		this.v31 = 0;
		this.v32 = 0;
		this.v33 = 1;
	}
	
	public float trace(){
		return this.v11 + this.v22 + this.v33;
	}
	
	/**
	 * Sets values of <code>this</code> matrix to values of supplied vectors
	 * @param u1 first vector
	 * @param u2 second vector
	 * @param u3 third vector
	 * @return <code>this</code> matrix
	 */
	public Matrix33f fromAxis(Vector3f u1, Vector3f u2, Vector3f u3){
		this.v11 = u1.getX();
		this.v21 = u1.getY();
		this.v31 = u1.getZ();
		this.v12 = u2.getX();
		this.v22 = u2.getY();
		this.v32 = u2.getZ();
		this.v13 = u3.getX();
		this.v23 = u3.getY();
		this.v33 = u3.getZ();
		
		return this;
	}
	
	/**
	 * Sets <code>this</code> matrix to rotation matrix
	 * @param angle of rotation
	 * @param axis of rotation
	 * @return <code>this</code> rotation matrix
	 */
	public Matrix33f fromAngleAxis(float angle, Vector3f axis){
		Matrix33f skewSym = new Matrix33f(0, axis.getZ(), -axis.getY(), -axis.getZ(), 0, axis.getX(), axis.getY(), -axis.getX(), 0);
		Matrix33f skewSymSqr = new Matrix33f();
		skewSym.multiplyRes(skewSym, skewSymSqr);
		
		this.identity();
		this.add(skewSym.multiply(FloatMath.sin(FloatMath.degToRad(angle))));
		this.add(skewSymSqr.multiply((1-FloatMath.cos(FloatMath.degToRad(angle)))));
		return this;
	}
	
	public Matrix33f fromAngles(float yaw, float pitch, float roll){
		float cosRoll = FloatMath.cos(FloatMath.degToRad(roll));
		float sinRoll = FloatMath.sin(FloatMath.degToRad(roll));
		float cosPitch = FloatMath.cos(FloatMath.degToRad(pitch));
		float sinPitch = FloatMath.sin(FloatMath.degToRad(pitch));
		float cosYaw = FloatMath.cos(FloatMath.degToRad(yaw));
		float sinYaw = FloatMath.sin(FloatMath.degToRad(yaw));
		
		this.v11 = cosRoll*cosYaw;
		this.v12 = cosRoll*sinYaw*sinPitch-sinRoll*cosPitch;
		this.v13 = cosRoll*sinYaw*cosPitch+sinRoll*sinPitch;
		this.v21 = sinRoll*cosYaw;
		this.v22 = sinRoll*sinYaw*sinPitch+cosRoll*cosPitch;
		this.v23 = sinRoll*sinYaw*cosPitch-cosRoll*sinPitch;
		this.v31 = -sinYaw;
		this.v32 = cosYaw*sinPitch;
		this.v33 = cosYaw*cosPitch;
		
		return this;
	}
	
	public float getV11() {
		return v11;
	}

	public void setV11(float v11) {
		this.v11 = v11;
	}

	public float getV12() {
		return v12;
	}

	public void setV12(float v12) {
		this.v12 = v12;
	}

	public float getV13() {
		return v13;
	}

	public void setV13(float v13) {
		this.v13 = v13;
	}

	public float getV21() {
		return v21;
	}

	public void setV21(float v21) {
		this.v21 = v21;
	}

	public float getV22() {
		return v22;
	}

	public void setV22(float v22) {
		this.v22 = v22;
	}

	public float getV23() {
		return v23;
	}

	public void setV23(float v23) {
		this.v23 = v23;
	}

	public float getV31() {
		return v31;
	}

	public void setV31(float v31) {
		this.v31 = v31;
	}

	public float getV32() {
		return v32;
	}

	public void setV32(float v32) {
		this.v32 = v32;
	}

	public float getV33() {
		return v33;
	}

	public void setV33(float v33) {
		this.v33 = v33;
	}
	
	public boolean equals(Matrix33f second){
		if(this.v11 != second.v11)
			return false;
		if(this.v12 != second.v12)
			return false;
		if(this.v13 != second.v13)
			return false;
		if(this.v21 != second.v21)
			return false;
		if(this.v22 != second.v22)
			return false;
		if(this.v23 != second.v23)
			return false;
		if(this.v31 != second.v31)
			return false;
		if(this.v32 != second.v32)
			return false;
		if(this.v33 != second.v33)
			return false;
		
		return true;
	}
	
	public String toString(){
		String string = "[ "+this.v11+" ; "+this.v12+" ; "+this.v13+" ]\n" +
						"[ "+this.v21+" ; "+this.v22+" ; "+this.v23+" ]\n" +
						"[ "+this.v31+" ; "+this.v32+" ; "+this.v33+" ]\n";
		return string;
	}
}
